package com.sfdk.utils.pathfinder {
import com.sfdk.utils.IntPoint;

import flash.display.BitmapData;

public final class AStar {
    // The dimensions of the entire map
    private var width:int;
    private var height:int;

    // Our start and goal nodes
    private var _start:AStarNode;
    private var _goal:AStarNode;

    // must be two-dimensional array containing AStarNodes;
    private var map:Vector.<Vector.<AStarNode>>;
    private var convexZone:Vector.<Vector.<int>>;
    private var _heatMap:Vector.<Vector.<int>>;

    // open set: nodes to be considered
    public var open:Array;

    // closed set: nodes not to consider anymore
    public var closed:Vector.<AStarNode>;

    // Euclidian is better, but slower
    private var dist:Function = distManhattan;

    private var supportDiagonals : Boolean;
    //private var dist:Function = distManhattan;

    // Diagonal moves span a larger distance
    private static const COST_ORTHOGONAL:Number = 1;
    private static const COST_DIAGONAL:Number = 1.414;


    /**
     *
     * @param     map        The map to be searched, will not be modified
     * @param    start    Guess what? The starting position!
     * @param    goal    This is where we want to end up.
     */
    function AStar(map:IAStarSearchable, start:IntPoint = null, goal:IntPoint = null, supportDiagonals : Boolean = false) {
        width = map.getWidth();
        height = map.getHeight();

        if (start != null)
            _start = new AStarNode(start.x, start.y);
        if (goal != null)
            _goal = new AStarNode(goal.x, goal.y);

        this.map = createMap(map);
        this.convexZone = createConvexZone(map);
        this.supportDiagonals = supportDiagonals;
        _heatMap = initHeatMap(map);
        fillHeatMap(_heatMap);
    }


    /**
     *
     *
     *         FIND PATH
     *
     *     Find the path!
     *
     *     @return    An array of IntPoints describing the resulting path
     *
     */
    public function solve(start:IntPoint = null, goal:IntPoint = null, testedGoal:Vector.<IntPoint> = null):Vector.<IntPoint> {
        if (testedGoal == null)
            testedGoal = new Vector.<IntPoint>();
        testedGoal.push(goal);
        if (start != null) {
            _start = new AStarNode(start.x, start.y);
            _goal = map[goal.x][goal.y];
        }
        trace("Starting to solve: " + _start + " to " + _goal);
        var newGoal:IntPoint;
        if (!_goal.walkable) {
            trace("The goal is not walkable...");
            newGoal = climbHeatMap(goal, goal, this.convexZone[start.x][start.y], testedGoal);
            if (newGoal == null) {
                trace("Cannot work from the heatmap");
                return null;
            }
            else
                return solve(start, newGoal, testedGoal);
        }
        if (this.convexZone[goal.x][goal.y] != this.convexZone[start.x][start.y]) {
            newGoal = climbHeatMap(goal, goal, this.convexZone[start.x][start.y], testedGoal);
            if (newGoal == null) {
                trace("Cannot work from the heatmap");
                return null;
            }
            else
                return solve(start, newGoal, testedGoal);

        }

        open = [];
        closed = new Vector.<AStarNode>();


        var node:AStarNode = _start;
        node.h = dist(_goal);
        open.push(node);

        var solved:Boolean = false;
        var i:int = 0;


        // Ok let's start
        while (!solved) {

            // This line can actually be removed
            if (i++ > 1000) throw new Error("Overflow");

            // Sort open list by cost

            open.sortOn("f", Array.NUMERIC);
            if (open.length <= 0) break;
            node = open.shift();
            closed.push(node);

            // Could it be true, are we there?
            if (node.x == _goal.x && node.y == _goal.y) {
                // We found a solution!
                solved = true;
                break;
            }

            //var neighbors : Vector.<AStarNode> = neighbors(node);
            var n:AStarNode;
            for each (n in neighbors(node)) {

                if ((open.indexOf(n) == -1) && (closed.indexOf(n) == -1)) {
                    open.push(n);
                    n.parent = node;
                    n.h = dist(n);
                    n.g = node.g;
                } else {
                    var f:Number = n.g + node.g + n.h;
                    if (f < n.f) {
                        n.parent = node;
                        n.g = node.g;
                    }
                }
            }
        }

        // The loop was broken,
        // see if we found the solution
        if (solved) {
            trace("Solved");
            // We did! Format the data for use.
            var solution:Vector.<IntPoint> = new Vector.<IntPoint>();
            // Start at the end...
            solution.push(new IntPoint(node.x, node.y));
            // ...walk all the way to the start to record where we've been...
            while (node.parent && node.parent != _start) {
                node = node.parent;
                solution.push(new IntPoint(node.x, node.y));
            }
            // ...and add our initial position.
            solution.push(new IntPoint(node.x, node.y));

            solution.reverse();
            solution.forEach(calculateDiff);
            var realSolution:Vector.<IntPoint> = new Vector.<IntPoint>();
            realSolution.push(new IntPoint(node.x, node.y));
            var k:int = 0;
            var j:int;
            while (j < (solution.length - 1)) {
                realSolution[k].add(solution[j]);
                if (solution[j].x != solution[j + 1].x || solution[j].y != solution[j + 1].y) {
                    realSolution[k + 1] = realSolution[k].clone();
                    k++
                }
                j++;
            }
            return realSolution;
        } else {
            // No solution found... :(
            // This might be something else instead
            // (like an array with only the starting position)
            return null;
        }
    }

    private function calculateDiff(currPoint:IntPoint, currIndex:int, vector:Vector.<IntPoint>):void {
        if (currIndex < vector.length - 1) {
            currPoint.x = vector[currIndex + 1].x - currPoint.x;
            currPoint.y = vector[currIndex + 1].y - currPoint.y;
        }
    }

    /**
     *     Faster, more inaccurate heuristic method
     */
    private function distManhattan(n1:AStarNode, n2:AStarNode = null):Number {
        if (n2 == null) n2 = _goal;
        return Math.abs(n1.x - n2.x) + Math.abs(n1.y - n2.y);
    }

    /**
     *     Slower but much better heuristic method. Actually,
     *     this returns just the distance between 2 points.
     */
    private function distEuclidian(n1:AStarNode, n2:AStarNode = null):Number {
        if (n2 == null) n2 = _goal;
        return Math.sqrt(Math.pow((n1.x - n2.x), 2) + Math.pow((n1.y - n2.y), 2));
    }


    /**
     *
     *         NEIGHBORS
     *
     *     Return a node's neighbors, IF they're walkable
     *
     *     @return An array of AStarNodes.
     */
    private function neighbors(node:AStarNode):Vector.<AStarNode> {
        var x:int = node.x;
        var y:int = node.y;
        var n:AStarNode;
        var a:Vector.<AStarNode> = new Vector.<AStarNode>();

        // N
        if (x > 0) {

            n = map[x - 1][y];
            if (n.walkable) {
                n.g += COST_ORTHOGONAL;
                a.push(n);
            }
        }
        // E
        if (x < width - 1) {
            n = map[x + 1][y];
            if (n.walkable) {
                n.g += COST_ORTHOGONAL;
                a.push(n);
            }
        }
        // N
        if (y > 0) {
            n = map[x][y - 1];
            if (n.walkable) {
                n.g += COST_ORTHOGONAL;
                a.push(n);
            }
        }
        // S
        if (y < height - 1) {
            n = map[x][y + 1];
            if (n.walkable) {
                n.g += COST_ORTHOGONAL;
                a.push(n);
            }
        }

        // Don't cut corners here,
        // but make diagonal travelling possible.
        if(supportDiagonals)
        {
            // NW
            if (x > 0 && y > 0) {
                n = map[x - 1][y - 1];
                if (n.walkable
                        && map[x - 1][y].walkable
                        && map[x][y - 1].walkable
                        ) {
                    n.g += COST_DIAGONAL;
                    a.push(n);
                }
            }
            // NE
            if (x < width - 1 && y > 0) {
                n = map[x + 1][y - 1];
                if (n.walkable
                        && map[x + 1][y].walkable
                        && map[x][y - 1].walkable
                        ) {
                    n.g += COST_DIAGONAL;
                    a.push(n);
                }
            }
            // SW
            if (x > 0 && y < height - 1) {
                n = map[x - 1][y + 1];
                if (n.walkable
                        && map[x - 1][y].walkable
                        && map[x][y + 1].walkable
                        ) {
                    n.g += COST_DIAGONAL;
                    a.push(n);
                }
            }
            // SE
            if (x < width - 1 && y < height - 1) {
                n = map[x + 1][y + 1];
                if (n.walkable
                        && map[x + 1][y].walkable
                        && map[x][y + 1].walkable
                        ) {
                    n.g += COST_DIAGONAL;
                    a.push(n);
                }
            }
        }

        return a;

    }

    /**
     *         CREATE MAP
     *
     * Create a map with cost and heuristic values for each tile
     *
     */
    private function createMap(map:IAStarSearchable):Vector.<Vector.<AStarNode>> {
        var a:Vector.<Vector.<AStarNode>> = new Vector.<Vector.<AStarNode>>(width);
        for (var x:int = 0; x < width; x++) {
            a[x] = new Vector.<AStarNode>(height);
            for (var y:int = 0; y < height; y++) {
                a[x][y] = new AStarNode(x, y, map.isWalkable(x, y));
            }
        }

        return a;
    }

    private function createConvexZone(map:IAStarSearchable):Vector.<Vector.<int>> {
        var x:int = 0;
        var y:int = 0;
        var colorWalk:uint = 0x000001;
        var colorUnWalk:uint = 0x000002;
        var image:BitmapData = new BitmapData(width, height, false);
        for (x = 0; x < width; x++) {
            for (y = 0; y < height; y++) {
                if (map.isWalkable(x, y))
                    image.setPixel(x, y, colorWalk);
                else
                    image.setPixel(x, y, colorUnWalk);
            }
        }
        var currentColor:uint = 0x000003;
        for (x = 0; x < width; x++) {
            for (y = 0; y < height; y++) {
                if (image.getPixel(x, y) == colorWalk) {
                    image.floodFill(x, y, currentColor);
                    currentColor++;
                }
            }
        }
        var a:Vector.<Vector.<int>> = new Vector.<Vector.<int>>(width);
        for (x = 0; x < width; x++) {
            a[x] = new Vector.<int>(height);
            for (y = 0; y < height; y++) {
                a[x][y] = image.getPixel(x, y);
            }
        }
        return a;
    }

    private function initHeatMap(map:IAStarSearchable):Vector.<Vector.<int>> {
        var heatMap:Vector.<Vector.<int>> = new Vector.<Vector.<int>>(width);
        var x:int, y:int;
        var foundOther:Boolean;
        var walkable:Boolean;
        for (x = 0; x < width; x++) {
            heatMap[x] = new Vector.<int>(height);
            for (y = 0; y < height; y++) {
                if (map.isWalkable(x, y))
                    heatMap[x][y] = -1;
                else {
                    foundOther = false;
                    if (x >= 1) {
                        walkable = map.isWalkable(x - 1, y);
                        if (walkable)
                            foundOther = true;
                    }
                    if (x < (width - 1)) {
                        walkable = map.isWalkable(x + 1, y);
                        if (walkable)
                            foundOther = true;
                    }
                    if (y >= 1) {
                        walkable = map.isWalkable(x, y - 1);
                        if (walkable)
                            foundOther = true;
                    }
                    if (y < (height - 1)) {
                        walkable = map.isWalkable(x, y + 1);
                        if (walkable)
                            foundOther = true;
                    }

                    if(supportDiagonals)
                    {
                        //Diagonal component
                        if (x >= 1 && y >= 1) {
                            walkable = map.isWalkable(x - 1, y - 1);
                            if (walkable)
                                foundOther = true;
                        }
                        if (x < (width - 1) && y >= 1) {
                            walkable = map.isWalkable(x + 1, y - 1);
                            if (walkable)
                                foundOther = true;
                        }
                        if (x >= 1 && y < (height - 1)) {
                            walkable = map.isWalkable(x - 1, y + 1);
                            if (walkable)
                                foundOther = true;
                        }
                        if (x < (width - 1) && y < (height - 1)) {
                            walkable = map.isWalkable(x + 1, y + 1);
                            if (walkable)
                                foundOther = true;
                        }
                    }
                    if (foundOther)
                        heatMap[x][y] = 0;
                    else
                        heatMap[x][y] = -1;
                }
            }
        }
        return heatMap;
    }

    private function fillHeatMap(heatMap:Vector.<Vector.<int>>, level:int = 0):void {
        var globalChanged:Boolean = false;
        var localChanged:Boolean;
        var x:int, y:int;
        for (x = 0; x < width; x++) {
            for (y = 0; y < height; y++) {
                if (heatMap[x][y] == level) {
                    localChanged = fillAdjacentTile(heatMap, x, y, level + 1);
                    if (localChanged)
                        globalChanged = true;
                }
            }
        }
        if (globalChanged)
            fillHeatMap(heatMap, level + 1);
    }

    private function fillAdjacentTile(heatMap:Vector.<Vector.<int>>, x:int, y:int, level:int):Boolean {
        var hasChanged:Boolean = false;
        var currentValue:int;
        if (x >= 1) {
            currentValue = heatMap[x - 1][y];
            if (currentValue == -1 || currentValue > level) {
                heatMap[x - 1][y] = level;
                hasChanged = true;
            }
        }
        if (x < (width - 1)) {
            currentValue = heatMap[x + 1][y];
            if (currentValue == -1 || currentValue > level) {
                heatMap[x + 1][y] = level;
                hasChanged = true;
            }
        }
        if (y >= 1) {
            currentValue = heatMap[x][y - 1];
            if (currentValue == -1 || currentValue > level) {
                heatMap[x][y - 1] = level;
                hasChanged = true;
            }
        }
        if (y < (height - 1)) {
            currentValue = heatMap[x][y + 1];
            if (currentValue == -1 || currentValue > level) {
                heatMap[x][y + 1] = level;
                hasChanged = true;
            }
        }

        if(supportDiagonals)
        {
            //Diagonal component
            if (x >= 1 && y >= 1) {
                currentValue = heatMap[x - 1][y - 1];
                if (currentValue == -1 || currentValue > level) {
                    heatMap[x - 1][y - 1] = level;
                    hasChanged = true;
                }
            }
            if (x < (width - 1) && y >= 1) {
                currentValue = heatMap[x + 1][y - 1];
                if (currentValue == -1 || currentValue > level) {
                    heatMap[x + 1][y - 1] = level;
                    hasChanged = true;
                }
            }
            if (x >= 1 && y < (height - 1)) {
                currentValue = heatMap[x - 1][y + 1];
                if (currentValue == -1 || currentValue > level) {
                    heatMap[x - 1][y + 1] = level;
                    hasChanged = true;
                }
            }
            if (x < (width - 1) && y < (height - 1)) {
                currentValue = heatMap[x + 1][y + 1];
                if (currentValue == -1 || currentValue > level) {
                    heatMap[x + 1][y + 1] = level;
                    hasChanged = true;
                }
            }
        }
        return hasChanged;
    }

    private function climbHeatMap(point:IntPoint, initPoint:IntPoint, initConvexZone:int, testedGoal:Vector.<IntPoint>):IntPoint {
        var x:int = point.x;
        var y:int = point.y;
        var newValue:int;
        var smallestValue:int = _heatMap[x][y];
        var smallestDirection:IntPoint;
        if (smallestValue != 0) {
            if (x > 1) {
                newValue = _heatMap[x - 1][y];
                if (newValue < smallestValue) {
                    smallestValue = newValue;
                    smallestDirection = new IntPoint(x - 1, y);
                }
            }
            if (x < (width - 1)) {
                newValue = _heatMap[x + 1][y];
                if (newValue < smallestValue) {
                    smallestValue = newValue;
                    smallestDirection = new IntPoint(x + 1, y);
                }
            }
            if (y > 1) {
                newValue = _heatMap[x][y - 1];
                if (newValue < smallestValue) {
                    smallestValue = newValue;
                    smallestDirection = new IntPoint(x, y - 1);
                }
            }
            if (y < (height - 1)) {
                newValue = _heatMap[x][y + 1];
                if (newValue < smallestValue) {
                    smallestValue = newValue;
                    smallestDirection = new IntPoint(x, y + 1);
                }
            }

            if(supportDiagonals)
            {
                //Diagonal component
                if (x > 1 && y > 1) {
                    newValue = _heatMap[x - 1][y - 1];
                    if (newValue < smallestValue) {
                        smallestValue = newValue;
                        smallestDirection = new IntPoint(x - 1, y - 1);
                    }
                    ;
                }
                if (x < (width - 1) && y > 1) {
                    newValue = _heatMap[x + 1][y - 1];
                    if (newValue < smallestValue) {
                        smallestValue = newValue;
                        smallestDirection = new IntPoint(x + 1, y - 1);
                    }
                }
                if (x > 1 && y < (height - 1)) {
                    newValue = _heatMap[x - 1][y + 1];
                    if (newValue < smallestValue) {
                        smallestValue = newValue;
                        smallestDirection = new IntPoint(x - 1, y + 1);
                    }
                }
                if (x < (width - 1) && y < (height - 1)) {
                    newValue = _heatMap[x + 1][y + 1];
                    if (newValue < smallestValue) {
                        smallestValue = newValue;
                        smallestDirection = new IntPoint(x + 1, y + 1);
                    }
                }
            }
            return climbHeatMap(smallestDirection, initPoint, initConvexZone, testedGoal);
        }
        else {
            var diffX:Number;
            var diffY:Number;
            if ((point.x - initPoint.x) == 0)
                diffX = 0;
            else
                diffX = (point.x - initPoint.x) / Math.abs(point.x - initPoint.x);
            if ((point.y - initPoint.y) == 0)
                diffY = 0;
            else
                diffY = (point.y - initPoint.y) / Math.abs(point.y - initPoint.y);

            if (diffX != 0 || diffY != 0) {
                var k:Number = 0.5;
                while (k < 100) {
                    newValue = _heatMap[int(point.x + k * diffX)][int(point.y + k * diffY)];
                    if (newValue == 1) {
                        var notTested:Boolean = true;
                        for each(var testedPoint:IntPoint in testedGoal) {
                            if (testedPoint.x == int(point.x + k * diffX) && testedPoint.y == int(point.y + k * diffY))
                                notTested = false;
                        }
                        if (notTested)
                            return new IntPoint(int(point.x + k * diffX), int(point.y + k * diffY));
                    }
                    k += 0.5;
                }
            }
            else {
                if (x > 1) {
                    newValue = _heatMap[x - 1][y];
                    if (newValue == 1 && convexZone[x - 1][y] == initConvexZone) {
                        return new IntPoint(x - 1, y);
                    }
                }
                if (x < (width - 1)) {
                    newValue = _heatMap[x + 1][y];
                    if (newValue == 1 && convexZone[x + 1][y] == initConvexZone) {
                        return new IntPoint(x + 1, y);
                    }
                }
                if (y > 1) {
                    newValue = _heatMap[x][y - 1];
                    if (newValue == 1 && convexZone[x][y - 1] == initConvexZone) {
                        return new IntPoint(x, y - 1);
                    }
                }
                if (y < (height - 1)) {
                    newValue = _heatMap[x][y + 1];
                    if (newValue == 1 && convexZone[x][y + 1] == initConvexZone) {
                        return new IntPoint(x, y + 1);
                    }
                }

                if(supportDiagonals)
                {
                    //Diagonal component
                    if (x > 1 && y > 1) {
                        newValue = _heatMap[x - 1][y - 1];
                        if (newValue == 1 && convexZone[x - 1][y - 1] == initConvexZone) {
                            return new IntPoint(x - 1, y - 1);
                        }
                    }
                    if (x < (width - 1) && y > 1) {
                        newValue = _heatMap[x + 1][y - 1];
                        if (newValue == 1 && convexZone[x + 1][y - 1] == initConvexZone) {
                            return new IntPoint(x + 1, y - 1);
                        }
                    }
                    if (x > 1 && y < (height - 1)) {
                        newValue = _heatMap[x - 1][y + 1];
                        if (newValue == 1 && convexZone[x - 1][y + 1] == initConvexZone) {
                            return new IntPoint(x - 1, y + 1);
                        }
                    }
                    if (x < (width - 1) && y < (height - 1)) {
                        newValue = _heatMap[x + 1][y + 1];
                        if (newValue == 1 && convexZone[x + 1][y + 1] == initConvexZone) {
                            return new IntPoint(x + 1, y + 1);
                        }
                    }
                }
            }
        }
        return null;
    }
}
}